#!/usr/bin/env python3

import rospy
import struct
import paho.mqtt.client as mqtt
from sensor_msgs.msg import NavSatFix

class MqttSubscriber(object):
    def __init__(self):
        self.mqtt_client = mqtt.Client()
        self.mqtt_client.on_connect = self.on_connect
        self.mqtt_client.on_message = self.on_message
        self.mqtt_client.connect("test.mosquitto.org", 1883, 60)
        self.mqtt_client.loop_start()

        self.gps_data_publisher = rospy.Publisher('/gps_dest', NavSatFix, queue_size=10)

    def on_connect(self, client, userdata, flags, rc):
        rospy.logwarn("[mqtt_to_ros]Connected to MQTT broker with result code %s", str(rc))
        client.subscribe("wpr_dest_gps")

    # 从MQTT网络接收数据，并发布到ROS网络中
    def on_message(self, client, userdata, mqtt_msg):
        # rospy.loginfo("Received message on topic %s:  %s", mqtt_msg.topic, mqtt_msg.payload)
        if mqtt_msg.topic == "wpr_dest_gps":
            #values = struct.unpack('fff', mqtt_msg.payload)
            payload_str = mqtt_msg.payload.decode().split(",")
            latitude = float(payload_str[0])
            longitude = float(payload_str[1])
            yaw = float(payload_str[2])
            ros_msg = NavSatFix()
            ros_msg.latitude = latitude
            ros_msg.longitude = longitude
            ros_msg.altitude = yaw
            rospy.loginfo("[mqtt_to_ros] 接收到MQTT服务器 GPS: ( %f , %f , %f)", ros_msg.latitude, ros_msg.longitude, ros_msg.altitude)
            self.gps_data_publisher.publish(ros_msg)

    def run(self):
        rospy.spin()

if __name__ == '__main__':
    rospy.init_node('mqtt_to_ros')
    mqtt_subscriber = MqttSubscriber()
    mqtt_subscriber.run()